#ifndef _GMG_H_
#define _GMG_H_

#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Dense>
#include <ctime>
#include <cmath>
#include <string>
#include "Equation.h"
#include "Mesh.h"
#include "function2.h"
#define POW2(a) (1 << a)

using namespace Eigen;
using namespace std;
typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::VectorXd RHS;
typedef Eigen::VectorXd Solution;
typedef std::string Method;

template <unsigned int DIM>
std::map<int, double> interpolateBoundaryValues(auto &_boundary_function, std::set<Dofs<DIM>> _boundary_dofs, std::vector<Point<DIM>> _vertex)
{
    std::map<int, double> boundary_values;
    for (auto &dofs : _boundary_dofs)
    {
        int index = dofs.getGlobalIndex();
        double p[DIM];
        for (int i = 0; i < DIM; i++)
            p[i] = dofs[i];
        double value;
        if (p[1] == _vertex[0][1])
            value = _boundary_function[0](p);
        else if (p[0] == _vertex[1][0])
            value = _boundary_function[1](p);
        else if (p[1] == _vertex[1][1])
            value = _boundary_function[2](p);
        else if (p[0] == _vertex[0][0])
            value = _boundary_function[3](p);
        else
            abort(3);
        boundary_values.insert({index, value});
    }
    return boundary_values;
}

class MultiGridMethod
{
protected:
    vector<SpMat> A;                         // 所有A矩阵
    vector<Solution> u;                      // 所有解向量
    vector<RHS> f;                           // 所有右端项
    vector<SpMat> P;                         // I2htoh插值算子
    vector<SpMat> R;                         // Ihto2h限制算子
    vector<vector<int>> indexOfBoundaryDofs; // 所有网格边界层的自由度编号
    int level;                               // 当前层数
    int maxlevel;                            // 最大层数
    int num;                                 // 松弛次数
    Eigen::ConjugateGradient<SpMat> Solver_sparse;

    Mesh<2> *mesh;

public:
    MultiGridMethod(){};
    ~MultiGridMethod(){};
    void setMaxLevel(int _n) { maxlevel = _n; }
    int getMaxLevel() { return maxlevel; }
    void setNum(int _n) { num = _n; }
    Solution getSolution() { return u[0]; };
    void getIndexOfBoundaryDofs(int _maxlevel, Mesh<2> *mesh);

    void DealWithBoundaryCondition(SpMat &_A, VectorXd &_Rhs);
    void V_Cycle(int _level);
    VectorXd &Solve();

    void GS(SpMat &_A, Solution &_u, RHS &_f);
    void Iterator(SpMat &_A, Solution &_u, RHS &_f, const Method &_m);
    void Smooth(SpMat &_A, Solution &_u, RHS &_f, const Method &_m, int _n);
};

void MultiGridMethod::Iterator(SpMat &_A, Solution &_u, RHS &_f, const Method &_m)
{
    if (_m.compare("GS") == 0)
        GS(_A, _u, _f);
}

void MultiGridMethod::Smooth(SpMat &_A, Solution &_u, RHS &_f, const Method &_m, int _n)
{
    for (int i = 0; i < _n; i++)
        Iterator(_A, _u, _f, _m);
}

void MultiGridMethod::DealWithBoundaryCondition(SpMat &_A, VectorXd &_Rhs)
{
    vector<Point<2>> p = {{-1.0, -1.0}, {1.0, -1.0}, {1.0, 1.0}, {-1.0, 1.0}};
    Domain<2> *domain = new Rectangle2D(p);
    int mesh_type = ((int)sqrt(mesh->getTotalNumDofs()) - 1) / mesh->getSegment()[0];
    vector<int> seg({mesh->getSegment()[0] / POW2(level - 1), mesh->getSegment()[1] / POW2(level - 1)});
    std::set<Dofs<2>> boundary_dofs;
    // boundary_dofs = mesh_t.getBoundaryDofs();
    if (mesh_type == 1)
    {
        Q1Mesh mesh_t(domain, seg);
        boundary_dofs = mesh_t.getBoundaryDofs();
    }
    else if (mesh_type == 2)
    {
        Q2Mesh mesh_t(domain, seg);
        boundary_dofs = mesh_t.getBoundaryDofs();
    }

    Equation<2> *equ = new PossionEquation<2>();
    equ->SetRightHandsTermFunction(fe);
    equ->SetBoundaryConditionFunction({g1, g2, g3, g4});

    // std::set<Dofs<2>> boundary_dofs;
    // boundary_dofs = mesh_t.getBoundaryDofs();
    std::map<int, double> boundary_values;
    auto boundary_function = equ->BoundaryConditionFunction();
    std::vector<Point<2>> vertex = {domain->getVertex(0), domain->getVertex(2)};
    boundary_values = interpolateBoundaryValues(boundary_function, boundary_dofs, vertex);
    // applyBoundaryValues(boundary_values, _A, _Rhs);

    for (auto &it : boundary_values)
    {
        // 边界自由度的全局编号。
        int k = it.first;
        // 对应自由的边界函数值。
        double bnd_value = it.second;
        if (level == 1)
            _Rhs[k] = bnd_value * _A.coeffRef(k, k);
        for (SpMat::InnerIterator it(_A, k); it; ++it)
        {
            int row = it.row();
            if (row == k)
                continue;
            if (level == 1)
                _Rhs[row] -= _A.coeffRef(k, row) * bnd_value;
            _A.coeffRef(k, row) = 0.0;
            _A.coeffRef(row, k) = 0.0;
        }
    }
}
void MultiGridMethod::V_Cycle(int _level)
{
    int index = 0;
    // int pow2_level = POW2(_level);
    vector<int> indexofBoundaryDofs = indexOfBoundaryDofs[_level];
    level = _level;
    SpMat A_h = A[level - 1];
    RHS f_h = f[level - 1];
    DealWithBoundaryCondition(A_h, f_h);
    if (level == maxlevel)
    {
        Solver_sparse.compute(A_h);
        u[level - 1] = Solver_sparse.solve(f_h);
    }
    else
    {
        Smooth(A_h, u[level - 1], f_h, "GS", 2);
        RHS r_2h = R[level - 1] * (f_h - A_h * u[level - 1]);
        // cout << R[level - 1] << endl;
        // cout << A[level - 1] << endl;
        // cout << A[level] << endl;
        for (int i = 0; i < r_2h.size(); i++)
            if (i == indexofBoundaryDofs[index])
            {
                r_2h[i] = 0.0;
                index++;
            }
        // for (int i = 0; i < r_2h.size(); i++)
        //     cout << r_2h[i] << endl;
        // cout << "------------------" << endl;
        Solution e_2h = MatrixXd::Zero(r_2h.size(), 1);
        u.push_back(e_2h);
        f.push_back(r_2h);
        level++;
        V_Cycle(level);
        level--;
        u[level - 1] = u[level - 1] + P[level - 1] * u[level];
        Smooth(A_h, u[level - 1], f_h, "GS", 2);
    }
}

void MultiGridMethod::GS(SpMat &_A, Solution &_u, RHS &_f)
{
    int index = 0;
    vector<int> indexofBoundaryDofs = indexOfBoundaryDofs[0];
    for (int i = 0; i < _u.size(); i++)
    {
        double f_i = _f[i];
        double a_ii = _A.coeffRef(i, i);
        if (i == indexofBoundaryDofs[index])
        {
            if (level == 1)
            {
                _u[i] = f_i / a_ii;
                index++;
                // for (int i = 0; i < _u.size(); i++)
                //     cout << i << "  " << _u[i] << endl;
                // cout << "hhhh" << endl;
            }
        }
        else
        {
            for (SpMat::InnerIterator it(_A, i); it; ++it)
            {
                int row = it.row();
                if (row == i)
                    continue;
                else
                    f_i -= (it.value() * _u[row]); // 把u写进函数参数，方便计算后自动更新u带入下一步计算
            }
            _u[i] = f_i / a_ii;
        }
    }
}

VectorXd &MultiGridMethod::Solve()
{
    std::cout << "Solve..." << std::endl;
    int times = 0;
    VectorXd residual_vector = A[0] * Eigen::MatrixXd::Zero(f[0].size(), 1) - f[0];
    // VectorXd residual_vector = _A_origin * Eigen::MatrixXd::Zero(_Rhs[0].size(), 1) - _Rhs_origin;
    double residual = residual_vector.norm();
    double residual_max = residual_vector.lpNorm<Eigen::Infinity>();
    do
    {
        V_Cycle(1);
        f.resize(1);
        u.resize(1);
        SpMat _A_temp = A[0];
        VectorXd _Rhs_temp = f[0];
        level = 1;
        DealWithBoundaryCondition(_A_temp, _Rhs_temp);
        residual_vector = _A_temp * u[0] - _Rhs_temp;
        residual = residual_vector.norm();
        residual_max = residual_vector.lpNorm<Eigen::Infinity>();
        std::cout << " the " << times + 1 << "th l2 norm of residual vecror is :" << residual << " and the max norm case is :" << residual_max << std::endl;
    } while (times++ < 20 & residual >= 1e-12);
    return u[0];
}

void MultiGridMethod::getIndexOfBoundaryDofs(int _maxlevel, Mesh<2> *mesh)
{
    int mesh_type = ((int)sqrt(mesh->getTotalNumDofs()) - 1) / mesh->getSegment()[0];
    // cout << mesh_type << "---" << endl;
    for (int i = 0; i < _maxlevel + 1; i++)
    {
        int pow2_level = POW2(i);
        vector<int> indexofBoundaryDofs;
        for (int i = 0; i < mesh->getSegment()[0] / pow2_level * mesh_type + 1; i++)
        {
            indexofBoundaryDofs.push_back(i);
            // indexofBoundaryDofs.push_back(i + (_mesh->getSegment()[0] + 1) * _mesh->getSegment()[1]);
        }
        for (int j = 1; j < mesh->getSegment()[1] / pow2_level * mesh_type; j++)
        {
            indexofBoundaryDofs.push_back(j * (mesh->getSegment()[0] / pow2_level * mesh_type + 1));
            indexofBoundaryDofs.push_back((j + 1) * (mesh->getSegment()[0] / pow2_level * mesh_type + 1) - 1);
        }
        for (int i = 0; i < mesh->getSegment()[0] / pow2_level * mesh_type + 1; i++)
            indexofBoundaryDofs.push_back(i + (mesh->getSegment()[0] / pow2_level * mesh_type + 1) * mesh->getSegment()[1] / pow2_level * mesh_type);
        indexOfBoundaryDofs.push_back(indexofBoundaryDofs);
    }
}

class Q1_MG : public MultiGridMethod
{
protected:
    typedef MultiGridMethod MG;
    typedef Eigen::Triplet<double> Tri;
    using MG::A;
    using MG::f;
    using MG::level;
    using MG::maxlevel;
    using MG::mesh;
    using MG::num;
    using MG::P;
    using MG::R;
    using MG::Solver_sparse;
    using MG::u;

public:
    Q1_MG(){};
    Q1_MG(SpMat &_A, RHS &_f, int _maxlevel, Mesh<2> *_mesh);
    ~Q1_MG(){};
    void reinit();
    void generatePR();
};

Q1_MG::Q1_MG(SpMat &_A, RHS &_f, int _maxlevel, Mesh<2> *_mesh)
{
    mesh = _mesh;
    level = 1;
    maxlevel = _maxlevel;
    A.push_back(_A);
    f.push_back(_f);
    u.push_back(Eigen::MatrixXd::Zero(f[0].size(), 1));
    A.resize(maxlevel);
    // u.resize(maxlevel);
    // f.resize(maxlevel);
    P.resize(maxlevel - 1);
    R.resize(maxlevel - 1);

    reinit();

    Solver_sparse.setTolerance(1e-12);
}

void Q1_MG::reinit()
{
    // A[level - 1] = _A;
    getIndexOfBoundaryDofs(maxlevel, mesh);
    generatePR();
    level++;
    while (level <= maxlevel)
    {
        A[level - 1] = R[level - 2] * A[level - 2] * P[level - 2];
        if (level != maxlevel)
            generatePR();
        level++;
    }
    level = 1;
}

void Q1_MG::generatePR()
{
    int n_h;
    if (level == 1)
        n_h = f[0].size();
    else
        n_h = P[level - 2].cols();
    int n_2h = ((sqrt(n_h) + 1) / 2) * ((sqrt(n_h) + 1) / 2);
    int x_2h = sqrt(n_2h);
    int y_2h = sqrt(n_2h);
    int x_h = sqrt(n_h);
    int y_h = sqrt(n_h);
    P[level - 1] = SpMat(n_h, n_2h);
    R[level - 1] = SpMat(n_2h, n_h);
    std::vector<Tri> TriList1(n_2h * 9);
    std::vector<Tri> TriList2(n_2h * 9);
    std::vector<Tri>::iterator it1 = TriList1.begin();
    std::vector<Tri>::iterator it2 = TriList2.begin();
    for (int i = 0; i < y_2h; i++)
        for (int j = 0; j < x_2h; j++)
        {
            clock_t start, end;
            start = clock();
            int CoarseIdx = j + x_2h * i;
            int FineIdx = 2 * j + x_h * 2 * i;
            *(it1++) = Tri(FineIdx, CoarseIdx, 1.0);
            *(it2++) = Tri(CoarseIdx, FineIdx, 1.0);
            if (i != 0)
            {
                *(it1++) = Tri(FineIdx - x_h, CoarseIdx, 0.5);
                *(it2++) = Tri(CoarseIdx, FineIdx - x_h, 0.5);
                if (j != 0)
                {
                    *(it1++) = Tri(FineIdx - x_h - 1, CoarseIdx, 0.25);
                    *(it2++) = Tri(CoarseIdx, FineIdx - x_h - 1, 0.25);
                }
                if (j != x_2h - 1)
                {
                    *(it1++) = Tri(FineIdx - x_h + 1, CoarseIdx, 0.25);
                    *(it2++) = Tri(CoarseIdx, FineIdx - x_h + 1, 0.25);
                }
            }
            if (i != y_2h - 1)
            {
                *(it1++) = Tri(FineIdx + x_h, CoarseIdx, 0.5);
                *(it2++) = Tri(CoarseIdx, FineIdx + x_h, 0.5);
                if (j != 0)
                {
                    *(it1++) = Tri(FineIdx + x_h - 1, CoarseIdx, 0.25);
                    *(it2++) = Tri(CoarseIdx, FineIdx + x_h - 1, 0.25);
                }
                if (j != x_2h - 1)
                {
                    *(it1++) = Tri(FineIdx + x_h + 1, CoarseIdx, 0.25);
                    *(it2++) = Tri(CoarseIdx, FineIdx + x_h + 1, 0.25);
                }
            }
            if (j != 0)
            {
                *(it1++) = Tri(FineIdx - 1, CoarseIdx, 0.5);
                *(it2++) = Tri(CoarseIdx, FineIdx - 1, 0.5);
            }
            if (j != x_2h - 1)
            {
                *(it1++) = Tri(FineIdx + 1, CoarseIdx, 0.5);
                *(it2++) = Tri(CoarseIdx, FineIdx + 1, 0.5);
            }
            end = clock();
            double time = (double)(end - start) / CLOCKS_PER_SEC;
            // cout << time << endl;
        }
    P[level - 1].setFromTriplets(TriList1.begin(), TriList1.end());
    P[level - 1].makeCompressed();
    R[level - 1].setFromTriplets(TriList2.begin(), TriList2.end());
    R[level - 1].makeCompressed();
}

class Q2_MG : public MultiGridMethod
{
protected:
    typedef MultiGridMethod MG;
    typedef Eigen::Triplet<double> Tri;
    using MG::A;
    using MG::f;
    using MG::level;
    using MG::maxlevel;
    using MG::mesh;
    using MG::num;
    using MG::P;
    using MG::R;
    using MG::Solver_sparse;
    using MG::u;

public:
    Q2_MG(){};
    Q2_MG(SpMat &_A, RHS &_f, int _maxlevel, Mesh<2> *_mesh);
    ~Q2_MG(){};
    void reinit();
    void generatePR();
};

Q2_MG::Q2_MG(SpMat &_A, RHS &_f, int _maxlevel, Mesh<2> *_mesh)
{
    mesh = _mesh;
    level = 1;
    maxlevel = _maxlevel;
    A.push_back(_A);
    f.push_back(_f);
    u.push_back(Eigen::MatrixXd::Zero(f[0].size(), 1));
    A.resize(maxlevel);
    // u.resize(maxlevel);
    // f.resize(maxlevel);
    P.resize(maxlevel - 1);
    R.resize(maxlevel - 1);
    reinit();
    Solver_sparse.setTolerance(1e-12);
}

void Q2_MG::reinit()
{
    // A[level - 1] = _A;
    getIndexOfBoundaryDofs(maxlevel, mesh);
    generatePR();
    level++;
    while (level <= maxlevel)
    {
        A[level - 1] = R[level - 2] * A[level - 2] * P[level - 2];
        if (level != maxlevel)
            generatePR();
        level++;
    }
    level = 1;
}

void Q2_MG::generatePR()
{
    int n_h;
    if (level == 1)
        n_h = f[0].size();
    else
        n_h = P[level - 2].cols();
    int n_2h = ((sqrt(n_h) + 1) / 2) * ((sqrt(n_h) + 1) / 2);
    int x_2h = sqrt(n_2h);
    int y_2h = sqrt(n_2h);
    int x_h = sqrt(n_h);
    int y_h = sqrt(n_h);
    P[level - 1] = SpMat(n_h, n_2h);
    R[level - 1] = SpMat(n_2h, n_h);
    std::vector<Tri> TriList1(n_2h * 25);
    std::vector<Tri> TriList2(n_2h * 25);
    std::vector<Tri>::iterator it1 = TriList1.begin();
    std::vector<Tri>::iterator it2 = TriList2.begin();
    for (int i = 0; i < y_2h; i++)
        for (int j = 0; j < x_2h; j++)
        {
            // idx = i + (_order * _segx + 1) * j;
            int CoarseIdx = j + (x_2h)*i;
            int FineIdx = 2 * j + (x_h)*2 * i;
            *(it1++) = Tri(FineIdx, CoarseIdx, 1.0);
            *(it2++) = Tri(CoarseIdx, FineIdx, 1.0);

            // 单元的四个角点
            if (i % 2 == 0 && j % 2 == 0)
            {
                // 不是下边界，下方有相关点
                if (i != 0)
                {
                    (*it1++) = Tri(FineIdx - x_h, CoarseIdx, 0.375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h, 0.375);
                    (*it1++) = Tri(FineIdx - 3 * x_h, CoarseIdx, -0.125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h, -0.125);
                    // 同时不是左边界， 左下方有相关点
                    if (j != 0)
                    {
                        (*it1++) = Tri(FineIdx - x_h - 1, CoarseIdx, 0.140625);
                        (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 1, 0.140625);
                        (*it1++) = Tri(FineIdx - x_h - 3, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 3, -0.046875);
                        (*it1++) = Tri(FineIdx - 3 * x_h - 1, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h - 1, -0.046875);
                        (*it1++) = Tri(FineIdx - 3 * x_h - 3, CoarseIdx, 0.015625);
                        (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h - 3, 0.015625);
                    }
                    // 同时不是右边界， 右下方有相关点
                    if (j != x_2h - 1)
                    {
                        (*it1++) = Tri(FineIdx - x_h + 1, CoarseIdx, 0.140625);
                        (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 1, 0.140625);
                        (*it1++) = Tri(FineIdx - x_h + 3, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 3, -0.046875);
                        (*it1++) = Tri(FineIdx - 3 * x_h + 1, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h + 1, -0.046875);
                        (*it1++) = Tri(FineIdx - 3 * x_h + 3, CoarseIdx, 0.015625);
                        (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h + 3, 0.015625);
                    }
                }
                // 不是上边界， 上方有相关点
                if (i != y_2h - 1)
                {
                    (*it1++) = Tri(FineIdx + x_h, CoarseIdx, 0.375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h, 0.375);
                    (*it1++) = Tri(FineIdx + 3 * x_h, CoarseIdx, -0.125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h, -0.125);
                    // 同时不是左边界， 左上方有相关点
                    if (j != 0)
                    {
                        (*it1++) = Tri(FineIdx + x_h - 1, CoarseIdx, 0.140625);
                        (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 1, 0.140625);
                        (*it1++) = Tri(FineIdx + x_h - 3, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 3, -0.046875);
                        (*it1++) = Tri(FineIdx + 3 * x_h - 1, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h - 1, -0.046875);
                        (*it1++) = Tri(FineIdx + 3 * x_h - 3, CoarseIdx, 0.015625);
                        (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h - 3, 0.015625);
                    }
                    // 同时不是右边界， 右上方有相关点
                    if (j != x_2h - 1)
                    {
                        (*it1++) = Tri(FineIdx + x_h + 1, CoarseIdx, 0.140625);
                        (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 1, 0.140625);
                        (*it1++) = Tri(FineIdx + x_h + 3, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 3, -0.046875);
                        (*it1++) = Tri(FineIdx + 3 * x_h + 1, CoarseIdx, -0.046875);
                        (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h + 1, -0.046875);
                        (*it1++) = Tri(FineIdx + 3 * x_h + 3, CoarseIdx, 0.015625);
                        (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h + 3, 0.015625);
                    }
                }
                // 不是左边界，左方有相关点
                if (j != 0)
                {
                    *(it1++) = Tri(FineIdx - 1, CoarseIdx, 0.375);
                    *(it2++) = Tri(CoarseIdx, FineIdx - 1, 0.375);
                    *(it1++) = Tri(FineIdx - 3, CoarseIdx, -0.125);
                    *(it2++) = Tri(CoarseIdx, FineIdx - 3, -0.125);
                }
                // 不是右边界，右方有相关点
                if (j != x_2h - 1)
                {
                    *(it1++) = Tri(FineIdx + 1, CoarseIdx, 0.375);
                    *(it2++) = Tri(CoarseIdx, FineIdx + 1, 0.375);
                    *(it1++) = Tri(FineIdx + 3, CoarseIdx, -0.125);
                    *(it2++) = Tri(CoarseIdx, FineIdx + 3, -0.125);
                }
            }

            // 单元左右边的中点
            if (i % 2 != 0 && j % 2 == 0)
            {
                // 上下方必有相关点
                (*it1++) = Tri(FineIdx - x_h, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx - x_h, 0.75);
                (*it1++) = Tri(FineIdx + x_h, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx + x_h, 0.75);
                // 不是左边界，左方有相关点
                if (j != 0)
                {
                    (*it1++) = Tri(FineIdx - x_h - 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 1, 0.28125);
                    (*it1++) = Tri(FineIdx + x_h - 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 1, 0.28125);
                    (*it1++) = Tri(FineIdx - x_h - 3, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 3, -0.09375);
                    (*it1++) = Tri(FineIdx + x_h - 3, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 3, -0.09375);
                    *(it1++) = Tri(FineIdx - 1, CoarseIdx, 0.375);
                    *(it2++) = Tri(CoarseIdx, FineIdx - 1, 0.375);
                    *(it1++) = Tri(FineIdx - 3, CoarseIdx, -0.125);
                    *(it2++) = Tri(CoarseIdx, FineIdx - 3, -0.125);
                }
                // 不是右边界，右方有相关点
                if (j != x_2h - 1)
                {
                    (*it1++) = Tri(FineIdx - x_h + 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 1, 0.28125);
                    (*it1++) = Tri(FineIdx + x_h + 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 1, 0.28125);
                    (*it1++) = Tri(FineIdx - x_h + 3, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 3, -0.09375);
                    (*it1++) = Tri(FineIdx + x_h + 3, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 3, -0.09375);
                    *(it1++) = Tri(FineIdx + 1, CoarseIdx, 0.375);
                    *(it2++) = Tri(CoarseIdx, FineIdx + 1, 0.375);
                    *(it1++) = Tri(FineIdx + 3, CoarseIdx, -0.125);
                    *(it2++) = Tri(CoarseIdx, FineIdx + 3, -0.125);
                }
            }
            // 单元上下边的中点
            if (i % 2 == 0 && j % 2 != 0)
            {
                // 左右方必有相关点
                (*it1++) = Tri(FineIdx - 1, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx - 1, 0.75);
                (*it1++) = Tri(FineIdx + 1, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx + 1, 0.75);
                // 不是下边界，下方有相关点
                if (i != 0)
                {
                    (*it1++) = Tri(FineIdx - x_h, CoarseIdx, 0.375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h, 0.375);
                    (*it1++) = Tri(FineIdx - 3 * x_h, CoarseIdx, -0.125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h, -0.125);
                    (*it1++) = Tri(FineIdx - x_h - 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 1, 0.28125);
                    (*it1++) = Tri(FineIdx - x_h + 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 1, 0.28125);
                    (*it1++) = Tri(FineIdx - 3 * x_h - 1, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h - 1, -0.09375);
                    (*it1++) = Tri(FineIdx - 3 * x_h + 1, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx - 3 * x_h + 1, -0.09375);
                }
                // 不是上边界，上方有相关点
                if (i != y_2h - 1)
                {
                    (*it1++) = Tri(FineIdx + x_h, CoarseIdx, 0.375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h, 0.375);
                    (*it1++) = Tri(FineIdx + 3 * x_h, CoarseIdx, -0.125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h, -0.125);
                    (*it1++) = Tri(FineIdx + x_h - 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 1, 0.28125);
                    (*it1++) = Tri(FineIdx + x_h + 1, CoarseIdx, 0.28125);
                    (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 1, 0.28125);
                    (*it1++) = Tri(FineIdx + 3 * x_h - 1, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h - 1, -0.09375);
                    (*it1++) = Tri(FineIdx + 3 * x_h + 1, CoarseIdx, -0.09375);
                    (*it2++) = Tri(CoarseIdx, FineIdx + 3 * x_h + 1, -0.09375);
                }
            }
            // 单元的中心
            if (i % 2 != 0 && j % 2 != 0)
            {
                // 四周必有相关点
                (*it1++) = Tri(FineIdx + x_h, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx + x_h, 0.75);
                (*it1++) = Tri(FineIdx - x_h, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx - x_h, 0.75);
                (*it1++) = Tri(FineIdx - 1, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx - 1, 0.75);
                (*it1++) = Tri(FineIdx + 1, CoarseIdx, 0.75);
                (*it2++) = Tri(CoarseIdx, FineIdx + 1, 0.75);
                (*it1++) = Tri(FineIdx + x_h + 1, CoarseIdx, 0.5625);
                (*it2++) = Tri(CoarseIdx, FineIdx + x_h + 1, 0.5625);
                (*it1++) = Tri(FineIdx + x_h - 1, CoarseIdx, 0.5625);
                (*it2++) = Tri(CoarseIdx, FineIdx + x_h - 1, 0.5625);
                (*it1++) = Tri(FineIdx - x_h + 1, CoarseIdx, 0.5625);
                (*it2++) = Tri(CoarseIdx, FineIdx - x_h + 1, 0.5625);
                (*it1++) = Tri(FineIdx - x_h - 1, CoarseIdx, 0.5625);
                (*it2++) = Tri(CoarseIdx, FineIdx - x_h - 1, 0.5625);
            }
        }
    // for (int i = 0; i < TriList1.size(); i++)
    //     std::cout << TriList1[i].row() << " " << TriList1[i].col() << " " << TriList1[i].value() << std::endl;
    P[level - 1].setFromTriplets(TriList1.begin(), TriList1.end());
    P[level - 1].makeCompressed();
    R[level - 1].setFromTriplets(TriList2.begin(), TriList2.end());
    R[level - 1].makeCompressed();
}

#else
#endif